#include <math.h>
#include <fstream> 
#include <string.h> 
#include "StdCalculator.h"

#include "itkImage.h"
#include "itkImageFileReader.h"
#include "itkShiftScaleImageFilter.h"
#include "itkImportImageFilter.h"
#include "itkBinaryThresholdImageFilter.h"
#include "itkRescaleIntensityImageFilter.h"
#include "itkCastImageFilter.h"
#include "itkImageFileWriter.h"
#include "itkImageRegionIteratorWithIndex.h"
#include "itkBinaryErodeImageFilter.h"
#include "itkBinaryDilateImageFilter.h"
#include "itkBinaryBallStructuringElement.h"
#include "itkBinaryThinningImageFilter.h"
#include "itkImageDuplicator.h"


typedef unsigned short PixelType; // pixel type for Phase image. Note: unsigned char is not enough
typedef float          PixelValueType;
typedef itk::Image< PixelType, 3 >         ImageType3D; // The format of input Phase image is "3D Analyze"
typedef itk::Image< PixelType, 2 >         ImageType2D; // some temporary image variable and all the output images are 2D
typedef itk::Image< PixelValueType, 3 >    VelocityImageType; // Just for temporary image storage 3D
typedef itk::Image< PixelValueType, 2 >    ValueImageType; // Just for temporary image calculation,Not for output
// unsigned char can't be used to store phase image

void CornerPointRemoval(int rows, int columns, ImageType2D::Pointer bwThinningImg);
int LengthOfBranch(ImageType2D::IndexType startPoint, ImageType2D::Pointer bwImg);
float Distance(float pixelSpacing, ImageType2D::IndexType pointLast, ImageType2D::IndexType pointNow);
int SkeletonExtraction(float pixelSpacing, ImageType2D::IndexType endPoint, ImageType2D::Pointer aortaThinningImg, ImageType2D::IndexType * skeletonContourS, float * skeletonDistS);
int FindNextPoint(ImageType2D::IndexType point, ImageType2D::Pointer bwImg, ImageType2D::IndexType *pointNext);
void RemovePointsFromImg(int nPoints, ImageType2D::IndexType * pointList, ImageType2D::Pointer bwImg);
ImageType2D::IndexType EndPointLocation(int rows, int columns, ImageType2D::Pointer aortaThinningImg);
void get_cross_sections(int nPointsOnSkeleton, ImageType2D::IndexType * contourIdx, ImageType2D::Pointer aortaImage, int *nPointOnSections, ImageType2D::IndexType ** pointListOnSections);
int maxArray1D(int length, float * Array);
int minArray1D(int length, float * Array);
void Nearest(int nPointList, float **pointList, float velocity, int *idx);
float time_from_2point(float *pointA, float *pointB, float velocity);
float time_for_velocity(int nPointList, float **tsvCurves ,float velocity);

int main( int argc, char **argv) {

	const int RescaleSlope = 2; // Constant
	const int RescaleIntercept =-4096; //Constant
	const float partitionRatio = 0.5;
	int strelSize = 3; // morphological structuring element, which will be adjusted later according to the image size, baseline: PixelSpacing =1.56 

	typedef itk::ImageFileReader<ImageType3D> ReaderType3D; // Import the Analyze format 3D-image
	ReaderType3D::Pointer reader3D = ReaderType3D::New();


	reader3D->SetFileName(argv[1]);
	int Slices=atoi(argv[2]);//<# of frames>
	int Rows=atoi(argv[3]);//<height of the images>
	int Columns=atoi(argv[4]);//<width of the images>
	int givenPoint[2];
	givenPoint[0]=atoi(argv[5]);//<xLabel >
	givenPoint[1]=Rows-atoi(argv[6]);//<yLabel, ITK take the bottom-left corner as origin >
	int upperThreshold=atoi(argv[7]);//<upper threshold, default: 80>
	int lowerThreshold=atoi(argv[8]);//<lower threshold, default: 60>
	float RepetitionTime=atof(argv[9]); //ms <RepetitionTime, default:13.1>
	float PixelSpacing=atof(argv[10]); //mm <PixelSpacing, default:1.56>
	int VENC = atoi(argv[11]); // cm/s <VENC, default:150>

	typedef itk::ImageFileWriter<ImageType2D> WriterType; // Output 2D-images, like: Correlation_Image.dcm, Correlation_Binary_Image.dcm, Segmented_Aorta.dcm, Segmented_Aorta_Thining.dcm, Aorta_Skeleton.dcm,
	WriterType::Pointer writer= WriterType::New();

	typedef itk::ShiftScaleImageFilter<ImageType3D, VelocityImageType > ShiftScaleFilterType;//Magnitude->Velocity: VENC
	ShiftScaleFilterType::Pointer shiftFilter = ShiftScaleFilterType::New(); 
	shiftFilter->SetInput( reader3D->GetOutput() );
	shiftFilter->SetScale( RescaleSlope*VENC/4096.0 );//We don't use RescaleSlope here to prevent int/int=int ~
	shiftFilter->SetShift( RescaleIntercept/RescaleSlope );
	shiftFilter->Update();

	VelocityImageType::Pointer velocityImage = shiftFilter->GetOutput();//Just for temporary 3D image storage



	// 1. Obtain the correlation image ****************************************

	// First, get velocity waveform sample (from the given point on ascending/descending aorta)
	VelocityImageType::IndexType pixelIndex3D;	
	pixelIndex3D[0] = givenPoint[0];  
	pixelIndex3D[1] = givenPoint[1]; //YLabel from bottom to top ^ 
	pixelIndex3D[2] =  0; //Non-useful, just taken as an iterative variable

	PixelValueType *velocityWaveformSample = new PixelValueType[Slices];
	for(pixelIndex3D[2]=0; pixelIndex3D[2] < Slices; pixelIndex3D[2]++)
	{
		velocityWaveformSample[pixelIndex3D[2]] = velocityImage->GetPixel(pixelIndex3D);
	}
	
	pixelIndex3D[0] = 0; // Reset for the following calculation of correlation
	pixelIndex3D[1] = 0; 
	pixelIndex3D[2] = 0; 

	
	// Correlation image Output Pipeline (buffer->imageFilter->scaleConversion->writer)

	// Second, construct a data buffer to store the calculated correlation value		
	const unsigned int  numberOfPixels =  Columns * Rows;
	PixelValueType * correlationBuffer = new PixelValueType[numberOfPixels]; //For Correlation Image Output
	PixelValueType * iCorrelationBuffer = correlationBuffer; //Keep the correlationBuffer point to beginning of buffer

	PixelValueType *iVelocityWaveform = new PixelValueType[Slices]; // Velocity Waveform at point pixelIndex3D
	StdCalculator calc; 
	for(pixelIndex3D[1] = Rows-1; pixelIndex3D[1] >= 0; pixelIndex3D[1]--) // row by row, From top to bottom
	{
		for(pixelIndex3D[0]=0; pixelIndex3D[0] < Columns; pixelIndex3D[0]++)// x
		{
			for(pixelIndex3D[2]=0; pixelIndex3D[2] < Slices; pixelIndex3D[2]++)
			{
				iVelocityWaveform[ pixelIndex3D[2] ] = velocityImage->GetPixel(pixelIndex3D);
			}
			calc.SetValues(iVelocityWaveform, velocityWaveformSample, Slices);
			*iCorrelationBuffer++ = fabs(calc.Calculate_Correlation());
		}
	}

	// Third, assign the constructed buffer to a imageFilter.
	typedef itk::ImportImageFilter< PixelValueType, 2 > ImportFilterType;//ValueImageType, this filter will be reused later
	ImportFilterType::Pointer importFilter = ImportFilterType::New();
	ImportFilterType::SizeType size;
	size[0] = Columns; // size along X- horizon direction
	size[1] = Rows; // size along Y- vertical direction
	ImportFilterType::IndexType start;
	start.Fill( 0 ); //Initialize
	ImportFilterType::RegionType region;
	region.SetIndex( start );
	region.SetSize( size );
	importFilter->SetRegion( region );
	const bool importFilterWillOwnTheBuffer = true;    
	importFilter->SetImportPointer( correlationBuffer, numberOfPixels, importFilterWillOwnTheBuffer );// need update?

	// Finally, rescale the pixel intensity to normal range for image storage
	typedef itk::RescaleIntensityImageFilter<ValueImageType, ImageType2D > RescaleFilterType;
	RescaleFilterType::Pointer rescaleFilter = RescaleFilterType::New();
	rescaleFilter->SetInput( importFilter->GetOutput() );//******
	rescaleFilter->SetOutputMinimum( 0 );
	rescaleFilter->SetOutputMaximum( 255 );
	writer->SetFileName( "Correlation_Image.dcm" );
	writer->SetInput(rescaleFilter->GetOutput());
	writer->Update();



	// 2. Get binary (Black & White) Image **********************************
	// Here we can also use the itk: ValueImageType::Pointer correlationImage = importFilter->GetOutput();
	PixelValueType * binaryBuffer = new PixelValueType[ numberOfPixels ];
	PixelValueType * iBinaryBuffer = binaryBuffer; //Keep the binaryBuffer point to start address of buffer
	iCorrelationBuffer = correlationBuffer; // pointer address reset
	for(int j=0; j < Rows; j++) // row by row
	{
		for(int i=0; i < Columns; i++)// x direction
		{
			if ( j < ceil(Rows*partitionRatio) )
				*iBinaryBuffer++= (*iCorrelationBuffer++) < (upperThreshold/100.0) ? 0 : 255;
			else
				*iBinaryBuffer++= (*iCorrelationBuffer++) < (lowerThreshold/100.0) ? 0 : 255;
		}
	}

	importFilter->SetImportPointer( binaryBuffer, numberOfPixels, importFilterWillOwnTheBuffer ); // Reuse the former defined filter to assign data butter
	typedef itk::CastImageFilter<ValueImageType, ImageType2D > CastFilterType;
	CastFilterType::Pointer castFilter = CastFilterType::New();
	castFilter->SetInput( importFilter->GetOutput() );	
	ImageType2D::Pointer correlationBinaryImage = castFilter->GetOutput();// The correlation Binary image, Just for the better code readability
	writer->SetFileName( "Correlation_Binary_Image.dcm" );
	writer->SetInput (correlationBinaryImage);
	writer->Update();



	// 3. Remove background by Morphology Operation ** BinaryImage->Erode->Dilate *********************
	typedef itk::BinaryBallStructuringElement< PixelType, 2  >  StructuringElementType;
	StructuringElementType  structuringElement;
	strelSize = int( strelSize*(0.7813/PixelSpacing) + 0.5 ) ; // Adjust the size of mophological operator, int executes "rounding"
	structuringElement.SetRadius( strelSize );  // Should be adjusted
	structuringElement.CreateStructuringElement();

	typedef itk::BinaryErodeImageFilter< ImageType2D, ImageType2D,  StructuringElementType >  ErodeFilterType;
	ErodeFilterType::Pointer  binaryErode1  = ErodeFilterType::New();
	binaryErode1->SetKernel( structuringElement );

	typedef itk::BinaryDilateImageFilter< ImageType2D, ImageType2D,  StructuringElementType > DilateFilterType;
	DilateFilterType::Pointer binaryDilate1 = DilateFilterType::New();
	binaryDilate1->SetKernel( structuringElement );

	binaryErode1->SetInput( correlationBinaryImage );
	binaryDilate1->SetInput( binaryErode1->GetOutput() );
	binaryErode1->SetErodeValue( 255 );
	binaryDilate1->SetDilateValue( 255 );
//	writer->SetFileName( "2Morphology1.dcm" );
//	writer->SetInput (binaryDilate1->GetOutput());
//	writer->Update(); // This image is not important


//	ImageType2D::Pointer aortaSegmentedImage = correlationBinaryImage;
	ImageType2D::Pointer aortaSegmentedImage = binaryDilate1->GetOutput();
	// 5. Aorta Thining **********************************************************
	typedef itk::BinaryThinningImageFilter< ImageType2D, ImageType2D >  ThinningFilterType;
	ThinningFilterType::Pointer  binaryThinning  = ThinningFilterType::New();
	binaryThinning->SetInput(aortaSegmentedImage); // segmented aorta image import
	typedef itk::RescaleIntensityImageFilter<ImageType2D, ImageType2D > RescaleFilterType2; // Actually the rescale process could be ommited, try it later
	RescaleFilterType2::Pointer rescaleFilter2 = RescaleFilterType2::New();
	ImageType2D::Pointer aortaThinningImage = binaryThinning->GetOutput();// 0-1 aorta thining image, Just for the better code readability
    rescaleFilter2->SetInput( aortaThinningImage );// 0-1 ->0-255
	rescaleFilter2->SetOutputMinimum( 0 );
	rescaleFilter2->SetOutputMaximum( 255 );
	writer->SetFileName( "Segmented_Aorta_Thining.dcm" );
	writer->SetInput(rescaleFilter2->GetOutput());
	writer->Update();

   // Corner removal
    CornerPointRemoval(Rows, Columns, aortaThinningImage);
//	writer->SetFileName( "Aorta_Skeleton.dcm" ); //Segmented_Aorta_Thining_Corner_Point_Removed
//	writer->SetInput(aortaThinningImage);
//	writer->Update();



	// 6. Manually Connection or Spline curve fitting



    // 7. Skeleton extraction ***********************************************************

	//  endPoint location (the lowest point on descending aorta) ************************
	ImageType2D::IndexType endPoint;
	endPoint = EndPointLocation(Rows, Columns, aortaThinningImage);

	// Skeleton contour tracing
	ImageType2D::IndexType * skeletonContour = new ImageType2D::IndexType[500];// Assume <=500 points, [column, row] ----contour begin at the startPoint
	float * skeletonDistance = new float[500];// distance from nowPoint to startPoint
	int nPointsOnSkeleton = SkeletonExtraction(PixelSpacing, endPoint, aortaThinningImage, skeletonContour, skeletonDistance);// number of points on skeleton contour

	// if the extracted skeleton is less than 80 pixels, try to find the next segment
	int SegNum=0;
	std::cout << "The endPoint on Segment is: " <<endPoint<< std::endl;
	std::cout << "The nPointsOnSkeleton on Segment " <<++SegNum<<" is: " << nPointsOnSkeleton<< std::endl;
	while(nPointsOnSkeleton<50)// This should be taken as a input parameter later
	{
		if(nPointsOnSkeleton==0)
		{
			std::cout << "No Skeleton Segment is available. Please check the Seed Point position!" << std::endl;
			return -1; // Image is empty
		}

		RemovePointsFromImg(nPointsOnSkeleton, skeletonContour, aortaThinningImage);
		endPoint = EndPointLocation(Rows, Columns, aortaThinningImage);
		std::cout << "The endPoint on Segment is: " <<endPoint<< std::endl;
		nPointsOnSkeleton = SkeletonExtraction(PixelSpacing, endPoint, aortaThinningImage, skeletonContour, skeletonDistance);
		std::cout << "The nPointsOnSkeleton on Segment " <<++SegNum<<" is: " << nPointsOnSkeleton<< std::endl;
	}


	// Output extracted skeleton image
	for(int i=0; i<nPointsOnSkeleton; i++)
		aortaThinningImage->SetPixel( skeletonContour[i], 2 ); 

	writer->SetFileName( "Aorta_Skeleton.dcm" );
	writer->SetInput(aortaThinningImage);
	writer->Update();

	// Output skeleton contour points to TXT **********************
	std::ofstream fSkeleton;
	fSkeleton.open( "Aorta-Skeleton.txt",std::ios::out );
	for(int i=0; i < nPointsOnSkeleton; i++)
	{
		fSkeleton <<skeletonContour[i] <<std::endl;
	}
	fSkeleton.flush();
	fSkeleton.close();


		return 0;
}


void CornerPointRemoval(int rows, int columns, ImageType2D::Pointer bwThinningImg)// I/O bwThinningImg
/* Remove the corner points, such as
   1           1             1 2 3
   X 1  and  1 X             4 i 5   */
{
	ImageType2D::IndexType iPoint,point4,point2,point5;

	for(iPoint[1]=rows-2; iPoint[1] >= 1; iPoint[1]--) // Row by row, and from bottom to top, leave the border aside
	{
		for(iPoint[0]=1; iPoint[0] < columns-1; iPoint[0]++)// x
		{   
			point2[0] = iPoint[0];
			point2[1] = iPoint[1]-1;

			point4[0] = iPoint[0]-1;
			point4[1] = iPoint[1];

			point5[0] = iPoint[0]+1;
			point5[1] = iPoint[1];

			if (   ( (bwThinningImg->GetPixel(point2)==1) && (bwThinningImg->GetPixel(point4)==1) )  ||  ( (bwThinningImg->GetPixel(point2)==1) && (bwThinningImg->GetPixel(point5)==1) )   )
				bwThinningImg->SetPixel(iPoint,0);	// Remove the corner point	
		}
	}
}


ImageType2D::IndexType EndPointLocation(int rows, int columns, ImageType2D::Pointer bwThinningImg)// Only input
/*	% Input:
            % rows                no comments
            % columns             no comments
            % aortaThinningImg    aorta thining black&white image. */
{
	ImageType2D::IndexType endPoint;

	ImageType2D::IndexType iPoint; // xlabel-->iPoint[0]; ylabel-->iPoint[1].
	bool flag4for = 0;
	for(iPoint[1]=rows-1; iPoint[1] >= 0; iPoint[1]--) // Row by row, and from bottom to top
	{
		if ( flag4for == 1 )
			break;

		for(iPoint[0]=0; iPoint[0] < columns; iPoint[0]++)// x
		{   
			if ( bwThinningImg->GetPixel(iPoint) == 1 )
			{
				endPoint[0] = iPoint[0];
				endPoint[1] = iPoint[1];
				flag4for = 1;
				break;				
			}
		}
	}

	return endPoint;
}


int SkeletonExtraction(float pixelSpacing, ImageType2D::IndexType endPoint, ImageType2D::Pointer aortaThinningImg, // Input
					   ImageType2D::IndexType * skeletonContourS, float * skeletonDistS) // Output
/*	% Input:
            % pixelSpacing        1 X 1                 mm
            % endPoint            1 X 2                 [column, row]
            % aortaThinningImg    Column X Row          Black & White aorta binary image
    % Output:
             % nPointsOnSkeleton    1 X 1                     "Return" the number of points on skeleton
             % skeletonContourS     nPointsOnSkeleton X 2     [column, row] 
			 % skeletonDistS        nPointsOnSkeleton X 1     distance from pointNow to startPoint */
{
	// Contour tracing (from endPoint to startPoint to get the location of every points on aorta skeleton contour)

	if (aortaThinningImg->GetPixel(endPoint) == 0) // Found one new point on aorta
	{
		return 0; // This is an empty image
	}

	ImageType2D::IndexType pointNow = endPoint;

	ImageType2D::IndexType skeletonContourE[500];// Assume <=500 points, [column, row]----contour begin at the endPoint
	skeletonContourE[0] = endPoint; 

	float skeletonDistE[500]; //  physical distance from pointNow to endPoint
	skeletonDistE[0] = 0;	

	int nPointsOnSkeleton = 1; // Once we get the accurate number of points on skeleton, we discard the assumption of 500 points.

	// Make a backup copy of aortaThinningImg in case it was destroyed--could be omitted
	typedef itk::ImageDuplicator< ImageType2D > DuplicatorType;
	DuplicatorType::Pointer duplicator = DuplicatorType::New();
	duplicator->SetInputImage(aortaThinningImg);
	duplicator->Update();
	ImageType2D::Pointer skeletonImg = duplicator->GetOutput();

	bool converged = false;
	while (converged==false)
	{
		RemovePointsFromImg(1, &pointNow, skeletonImg);// Remove the current point from the skeleton
		ImageType2D::IndexType pointNext[7]; // since we assume the 8-connectivity.
		int nBranches = FindNextPoint(pointNow, skeletonImg, pointNext);

		if (nBranches==0) // It's already the last point
			converged = true;

		else if (nBranches==1) // just follow the trunk
		{
			//                                                                      distance between pointLast and pointNow
			skeletonDistE[nPointsOnSkeleton] = skeletonDistE[nPointsOnSkeleton-1] + Distance(pixelSpacing, pointNow, pointNext[0]);// distance(cm) from pointNow to endPoint

			pointNow = pointNext[0];        
			skeletonContourE[nPointsOnSkeleton] = pointNow;

			nPointsOnSkeleton = nPointsOnSkeleton + 1;
		}

		else // We meet the branches
		{
			RemovePointsFromImg(nBranches, pointNext, skeletonImg);// Remove all the branch start points from the skeleton

			float *branchLength = new float[nBranches]; // in pixels

			for (int i=0; i<nBranches; i++)
				branchLength[i] = LengthOfBranch(pointNext[i], skeletonImg);// skeletonImg should be remain the same after calling this function

			int trunkIdx = maxArray1D(nBranches, branchLength);//  the longest branch is the trunk
			delete branchLength;

			//                                                              distance between pointLast and pointNow
			skeletonDistE[nPointsOnSkeleton] = skeletonDistE[nPointsOnSkeleton-1] + Distance(pixelSpacing, pointNow, pointNext[trunkIdx]); // distance(cm) from pointNow to endPoin

			pointNow = pointNext[trunkIdx];
			skeletonContourE[nPointsOnSkeleton] = pointNow;

			nPointsOnSkeleton = nPointsOnSkeleton + 1;
		}

	}

	// startPoint correction
	// skeletonContourS:--startPoint-->endPoint
	for (int i=0; i<nPointsOnSkeleton; i++) // Calculate the pulse arrival time later (When the cross_sections are extracted)
	{
		skeletonContourS[i] = skeletonContourE[nPointsOnSkeleton-1-i]; // start-> <-end
		skeletonDistS[i] = skeletonDistE[nPointsOnSkeleton-1] - skeletonDistE[nPointsOnSkeleton-1-i];
	}

	return nPointsOnSkeleton;
}


int LengthOfBranch(ImageType2D::IndexType startPoint, ImageType2D::Pointer bwImg)
/*% Actually what we are calculating is just the number of pixels
% I don't think it's necessary to calculate the real phisical length here
% Our aim is just for branch removal

% point is the start point of the branch */
{
	int length = 1;
	ImageType2D::IndexType pointNow = startPoint;

	// Make a backup copy of bwImg in case it was destroyed, Since we just measure the branch length here.
	typedef itk::ImageDuplicator< ImageType2D > DuplicatorType;
	DuplicatorType::Pointer duplicator = DuplicatorType::New();
	duplicator->SetInputImage(bwImg);
	duplicator->Update();
	ImageType2D::Pointer bwBranchImg = duplicator->GetOutput();

	bool converged = false;

	while(converged==false)
	{
		RemovePointsFromImg(1, &pointNow, bwBranchImg);// Remove the current point from the skeleton
		ImageType2D::IndexType pointNext[7]; // since we assume the 8-connectivity.
		int nBranches = FindNextPoint(pointNow, bwBranchImg, pointNext);

		if (nBranches==0) // It's already the last point
			converged = true;

		else if (nBranches==1) // just follow the trunk
		{
			pointNow = pointNext[0];        
			length = length + 1;
		}

		else // We meet the branches
		{
			float *branchLength = new float[nBranches]; // in pixels

			RemovePointsFromImg(nBranches, pointNext, bwBranchImg);// Remove all the branch start points from the skeleton

			for (int i=0; i<nBranches; i++)
				branchLength[i] = LengthOfBranch(pointNext[i], bwBranchImg);

			int trunkIdx = maxArray1D(nBranches, branchLength);//  the longest branch is the trunk

			length = length + branchLength[trunkIdx]; // int-float conversion
			converged = true; // Really?
			delete branchLength;
		}
	}

	return length;
}


void RemovePointsFromImg(int nPoints, ImageType2D::IndexType * pointList, ImageType2D::Pointer bwImg)// Only input
/* Input:
         % nPoints     Number of points to be removed
         % pointList   The list of points (row and column location) to be removed
         % bwImg       The working image */
{
	for(int i=0; i<nPoints; i++)
		bwImg->SetPixel( pointList[i], 0 ); // Remove all the branch start points from the skeleton
}


int FindNextPoint(ImageType2D::IndexType pointNow, ImageType2D::Pointer bwImg,// Input
				  ImageType2D::IndexType *pointNext)//Output
/* Input:
         % pointNow    Current point--locates at '0'
		 % bwImg       The working image
   Output:
          % nBranches   "Return" Number of branches stem from the current point
		  % pointNext   The start points of all the subsequent branches, which should be allocated before call this function
		  %             ImageType2D::IndexType pointNext[7]; // since we assume the 8-connectivity.
    %  1 2 3
    %  4 0 5
    %  6 7 8          */    
{
	int nBranches = 0;
	ImageType2D::IndexType pointTmp; // Just for tempory use

	int Rows = bwImg->GetLargestPossibleRegion().GetSize()[1];// Revised on Jan 23rd
    int Columns = bwImg->GetLargestPossibleRegion().GetSize()[0];// Revised on Jan 23rd

	for(int c=-1;c<=+1;c++) // Column
	{
		for(int r=-1;r<=+1;r++) // Row
		{
			pointTmp[0] = pointNow[0]+c; 
			pointTmp[1] = pointNow[1]+r; 

			if (pointTmp[0]>Columns || pointTmp[1]>Rows) // Revised on Jan 23rd
			{
                continue;
			} // Revised on Jan 23rd

			if (bwImg->GetPixel(pointTmp) == 1) // Found one new point on aorta
			{                    
				pointNext[nBranches] = pointTmp;
				nBranches = nBranches + 1;
			}
		}
	}

	return nBranches;
}


float Distance(float pixelSpacing, ImageType2D::IndexType pointLast, ImageType2D::IndexType pointNow)// Only input
// calculate the physical length between two adjacent points
// pixelSpacing is in mm
// len is in cm
{
	float len;
	len = 0.1*pixelSpacing*sqrt( float(abs(pointNow[0]-pointLast[0]) + abs(pointNow[1]-pointLast[1])) );
	return len;
}


void get_cross_sections(int nPointsOnSkeleton, ImageType2D::IndexType * contourIdx, ImageType2D::Pointer aortaImage,//Input
						int *nPointOnSections, ImageType2D::IndexType ** pointListOnSections)// Output
						/*    % 3-neighbour Gradient Method
						% Input:
						% nPointsOnSkeleton                     number of points on aorta skeleton contour
						% contourIdx                   point list on aorta skeleton contour, 2 X nSkeleton, row col
						% aortaImage                   segmented aorta black&white image.
						%
						% Output:
						% pointListOnSections                point list on aorta cross sections, nSkeleton X 2(2maxRadius+1), row col row col ...
						% nPointsOnSections            number of points on sections, nSkeleton X 1 */
{
	//int width = aortaImage->GetLargestPossibleRegion().GetSize()[0];
	//int heigth = aortaImage->GetLargestPossibleRegion().GetSize()[1];// % bug fixed 0930

	const int maxRadius=5; //% The maximum number of points on each sections should be less than 11

	int nSkeleton = nPointsOnSkeleton; // % number of points on skeleton
	float tangent;
	float xDist,yDist;

	ImageType2D::IndexType crossSectionIdx[2*maxRadius+1];// Along the tangent line, 10 points on each side
	for (int i=0; i<nSkeleton; i++)
	{
		if(i==0)//% Take forward differences on start and end edges
		{
			xDist = contourIdx[1][0] - contourIdx[0][0];
			yDist = contourIdx[1][1] - contourIdx[0][1];
		}
		else if(i==nSkeleton-1)//% Take forward differences on start and end edges
		{
			xDist = contourIdx[nSkeleton-1][0] - contourIdx[nSkeleton-2][0];
			yDist = contourIdx[nSkeleton-1][1] - contourIdx[nSkeleton-2][1];
		}
		else//% Take centered differences on interior points
		{
			xDist = contourIdx[i+1][0] - contourIdx[i-1][0];
			yDist = contourIdx[i+1][1] - contourIdx[i-1][1];
		}


		if(yDist == 0)//same x
			// tangent needn't to be calculated
			for( int j=0; j<2*maxRadius+1; j++)
			{
				crossSectionIdx[j][0] = contourIdx[i][0];//same x
				crossSectionIdx[j][1] = contourIdx[i][1]-maxRadius+j;
			}		
		else
		{
			tangent = -xDist/yDist;

			//	std::cout << contourIdx[i] << std::endl;
			//	std::cin>>pause;

			for( int j=0; j<2*maxRadius+1; j++)
			{
				crossSectionIdx[j][0] = contourIdx[i][0]-maxRadius+j;// Known x
				// yt = gradient*(xt - x0) + y0; %Tangent line
				crossSectionIdx[j][1] = ceil(tangent*(-maxRadius+j)) + contourIdx[i][1];// Calculate y
			}	
		}

		//% And(Cross_Section, segmentedAortaBwimg) to get the cross section point location list
		//% But does it make sense to average the pulse arrival time of all points on
		//% the same cross section?
		int nPoints = 0;
		for( int k=0; k<2*maxRadius+1; k++)
		{
			if ( (crossSectionIdx[k][0]>=0) && (crossSectionIdx[k][0]<aortaImage->GetLargestPossibleRegion().GetSize()[0])
				&& (crossSectionIdx[k][1]>=0) && (crossSectionIdx[k][1]<aortaImage->GetLargestPossibleRegion().GetSize()[1]) )// % bug fixed 0930
			{
				if (aortaImage->GetPixel(crossSectionIdx[k])==255)
				{
					pointListOnSections[i][nPoints]=crossSectionIdx[k];
					nPoints++;
				}
//				else if (nPoints > 0) //% Prevent the tangent line cross two sections of aorta
//					break;
			}// % 0930
		}
		nPointOnSections[i] = nPoints;  

	}

}


int maxArray1D(int length, float * Array)
{
	int maxIdx = 0;
	float maxValue=Array[maxIdx];
	for(int i=0;i<length;i++) 
		if(Array[i]>maxValue)
		{
			maxIdx=i; 
			maxValue=Array[maxIdx];
		}
		return  maxIdx;
}


int minArray1D(int length, float * Array)
{
	int minIdx = 0;
	float minValue=Array[minIdx];
	for(int i=0;i<length;i++) 
		if(Array[i]<minValue)
		{
			minIdx=i;
			minValue=Array[minIdx];
		}
		return  minIdx;
}


void Nearest(int nPointList, float **pointList, float velocity,// Input
			 int *idx)//Output
{
	int minIdx,maxIdx;
	minIdx=0;
	maxIdx=0;
	for (int i=1; i<nPointList; i++) // start from 2nd point to prevent minIdx =-1
	{
		if(pointList[i][1]==velocity)
		{
			minIdx = i;
			maxIdx = i; 
			break;
		}
		else if (pointList[i][1]>velocity)// Climing the mountain
		{           
			minIdx = i-1;// believe me, i can't be less than 1 if the curve is smooth enough.
			maxIdx = i;
			break;
		}
	}

	idx[0]=minIdx;
	idx[1]=maxIdx;
}


float time_from_2point(float *pointA, float *pointB, float velocity)
{
	float time4Velocity;

	if (pointB[1]==pointA[1]) // y2 == y1
		time4Velocity = (pointB[1] + pointA[1])/2.0; // (x2+x1)/2
	else 	
		time4Velocity = (velocity-pointA[1]) * (pointB[0]-pointA[0])/(pointB[1]-pointA[1]) + pointA[0];// x = (y-y1)*(x2-x1)/(y2-y1) + x1	

	return time4Velocity;
}


float time_for_velocity(int nPointList, float **tsvCurves ,float velocity)// import Slices*****FFDSFSDF
// tsvCurves[Slices][2] --time shifted velocity curves
// locate the time point of the corresponding velocity
{
	float time=0;
	int nearestIdx[2];
	Nearest(nPointList, tsvCurves, velocity, nearestIdx);// nearestIdx[0]--Minimum one of the nearest index; nearestIdx[1]--Maximum one of the nearest index.

	if (nearestIdx[0]==nearestIdx[1])// velocity is luck on the curve points
		time = tsvCurves[nearestIdx[0]][0];
	else
		time = time_from_2point( tsvCurves[nearestIdx[0]], tsvCurves[nearestIdx[1]], velocity);

	return time;
}
